from __future__ import division

import os
import rospy
import roslib
roslib.load_manifest('rqt_biped_robin_driver')

from rqt_gui_py.plugin import Plugin
from python_qt_binding import loadUi
from python_qt_binding.QtCore import Qt, QTimer, Slot
from python_qt_binding.QtGui import QKeySequence, QShortcut, QWidget

from geometry_msgs.msg import Twist
from std_srvs.srv import Empty, EmptyRequest
from biped_robin_msgs.srv import SetModeService
from biped_robin_msgs.msg import BipedRobinGlobalBehaviour

class BipedRobinServicePlugin(Plugin):
    def __init__(self, context):
        super(BipedRobinServicePlugin, self).__init__(context)
        self.setObjectName('BipedRobinService')

        self._widget = QWidget()
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BipedRobinService.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('BipedRobinServiceUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
        self._widget.ack_push_button.pressed.connect(self._on_ack_pressed)
        self._widget.reference_push_button.pressed.connect(self._on_reference_pressed)
        self._widget.resetInternalState_push_button.pressed.connect(self._on_reset_pressed)
        self._widget.mode0_push_button.pressed.connect(self._on_mode0_pressed)
        self._widget.mode1_push_button.pressed.connect(self._on_mode1_pressed)
        self._widget.mode2_push_button.pressed.connect(self._on_mode2_pressed)
        self._widget.mode3_push_button.pressed.connect(self._on_mode3_pressed)
        self._widget.mode8_push_button.pressed.connect(self._on_mode8_pressed)
        self._widget.mode9_push_button.pressed.connect(self._on_mode9_pressed)
        
        rospy.wait_for_service('reference_srv')
        self._reference_srv = rospy.ServiceProxy('reference_srv', Empty)
        rospy.wait_for_service('acknowledge_srv')
        self._acknowledge_srv = rospy.ServiceProxy('acknowledge_srv', Empty)
        rospy.wait_for_service('stop_srv')
        self._stop_srv = rospy.ServiceProxy('stop_srv', Empty)
        rospy.wait_for_service('resetState_srv')
        self._reset_srv = rospy.ServiceProxy('resetState_srv', Empty)
        rospy.wait_for_service('setMode_srv')
        self._setMode_srv = rospy.ServiceProxy('setMode_srv', SetModeService)
        
        self._globalRobotBehaviour_sub = rospy.Subscriber('bipedRobinGlobalBehaviour', BipedRobinGlobalBehaviour, self._message_callback)
        self._globalRobotBehaviour = BipedRobinGlobalBehaviour()
        self._globalRobotBehaviour.areJointErrors = 0
        self._globalRobotBehaviour.areJointsReferenced = 0
        self._globalRobotBehaviour.areJointTowErrorsSmall = 0
        self._globalRobotBehaviour.areJointsReadyToDrive = 0
        self._globalRobotBehaviour.areForceSensorsOK = 0
        self._globalRobotBehaviour.isAirborne = 0
        self._globalRobotBehaviour.areJointsReadyToDriveAirborne = 0 
        self._globalRobotBehaviour.isReadyToWalk = 0
        self._globalRobotBehaviour.currentMotionMode = 0
        
        # init and start update timer with 100ms
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self._update_timeout)
        self._update_timer.start(100)

    def _message_callback(self, data):
        self._globalRobotBehaviour.areJointErrors = data.areJointErrors
        self._globalRobotBehaviour.areJointsReferenced = data.areJointsReferenced
        self._globalRobotBehaviour.areJointTowErrorsSmall = data.areJointTowErrorsSmall
        self._globalRobotBehaviour.areJointsReadyToDrive = data.areJointsReadyToDrive
        self._globalRobotBehaviour.areForceSensorsOK = data.areForceSensorsOK
        self._globalRobotBehaviour.isAirborne = data.isAirborne
        self._globalRobotBehaviour.areJointsReadyToDriveAirborne = data.areJointsReadyToDriveAirborne
        self._globalRobotBehaviour.isReadyToWalk = data.isReadyToWalk
        self._globalRobotBehaviour.currentMotionMode = data.currentMotionMode

    def _update_timeout(self):
        if self._globalRobotBehaviour.currentMotionMode == 0:
            self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: blue}')
        else:
            self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: grey}')
            
        if self._globalRobotBehaviour.currentMotionMode == 1:
            self._widget.mode1_push_button.setStyleSheet('QPushButton {background-color: blue}')
        else:
            self._widget.mode1_push_button.setStyleSheet('QPushButton {background-color: grey}')
            
        if self._globalRobotBehaviour.currentMotionMode == 2:
            self._widget.mode2_push_button.setStyleSheet('QPushButton {background-color: blue}')
        else:
            self._widget.mode2_push_button.setStyleSheet('QPushButton {background-color: grey}')
            
        if self._globalRobotBehaviour.currentMotionMode == 3:
            self._widget.mode3_push_button.setStyleSheet('QPushButton {background-color: yellow}')
        else:
            self._widget.mode3_push_button.setStyleSheet('QPushButton {background-color: grey}')
 
        if self._globalRobotBehaviour.currentMotionMode == 8:
            self._widget.mode8_push_button.setStyleSheet('QPushButton {background-color: yellow}')
        else:
            self._widget.mode8_push_button.setStyleSheet('QPushButton {background-color: grey}')

        if self._globalRobotBehaviour.currentMotionMode == 9:
            self._widget.mode9_push_button.setStyleSheet('QPushButton {background-color: yellow}')
        else:
            self._widget.mode9_push_button.setStyleSheet('QPushButton {background-color: grey}')
           
        if self._globalRobotBehaviour.areJointErrors:
            self._widget.ack_push_button.setStyleSheet('QPushButton {background-color: red}')
        else:
            self._widget.ack_push_button.setStyleSheet('QPushButton {background-color: green}')

        if self._globalRobotBehaviour.areJointsReferenced:
            self._widget.reference_push_button.setStyleSheet('QPushButton {background-color: green}')
        else:
            self._widget.reference_push_button.setStyleSheet('QPushButton {background-color: red}')
            
        if self._globalRobotBehaviour.areJointTowErrorsSmall:
            self._widget.resetInternalState_push_button.setStyleSheet('QPushButton {background-color: green}')
        else:
            self._widget.resetInternalState_push_button.setStyleSheet('QPushButton {background-color: red}')
            if self._globalRobotBehaviour.currentMotionMode != 0:
                self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: red}')

    def _on_stop_pressed(self):
        self._stop_srv(EmptyRequest())

    def _on_ack_pressed(self):
        self._acknowledge_srv(EmptyRequest())

    def _on_reference_pressed(self):
        self._reference_srv(EmptyRequest())

    def _on_reset_pressed(self):
        self._reset_srv(EmptyRequest())

    def _on_mode0_pressed(self):
        self._setMode_srv(0)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: green}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: black}')

    def _on_mode1_pressed(self):
        self._setMode_srv(1)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: green}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: black}')

    def _on_mode2_pressed(self):
        self._setMode_srv(2)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: green}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: black}')

    def _on_mode3_pressed(self):
        self._setMode_srv(3)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: green}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: black}')

    def _on_mode8_pressed(self):
        self._setMode_srv(8)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: green}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: black}')

    def _on_mode9_pressed(self):
        self._setMode_srv(9)
        self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode8_push_button.setStyleSheet('QPushButton {color: black}')
        self._widget.mode9_push_button.setStyleSheet('QPushButton {color: green}')


    def shutdown_plugin(self):
        self._globalRobotBehaviour_sub.unregister()
        self._update_timer.stop()
        

    def save_settings(self, plugin_settings, instance_settings):
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        pass
